#include "motor_28byj48.h"


void Motor_28BYJ48_Init(
    Motor_28BYJ48_t *motor ,
    volatile unsigned int * _port ,     
    char          first_pin_pos  ,
    uint16_t  speed 
)  
{
    motor->port = _port;
    motor->speed = 65535 - speed ;
    motor->x = 0 ;
    motor->LUT = 0x8C462319 ;
    motor->mask = 0xfu << first_pin_pos ;
    
}

void Motor_28BYJ48_Walk(Motor_28BYJ48_t *motor,int32_t step ) {
#if (MOTOR_28BYJ48_MODE == 1)
    motor->willPass += step ;
#else
    int32_t absStep = step<0 ? -step : step ;
    for(int32_t i=0;i<absStep;++i) {
        *motor->port = (*motor->port & (~motor->mask) ) | (motor->LUT & motor->mask) ;
        if(step<0) {
            motor->LUT = (motor->LUT>>28) | (motor->LUT<<4) ;
        } else {
            motor->LUT = (motor->LUT<<28) | (motor->LUT>>4) ;
        }
        Serv_DelayTik(motor->speed) ;
    }
    motor->x += step;
#endif 
}

#if (MOTOR_28BYJ48_MODE == 1)
void Motor_28BYJ48_Yield(Motor_28BYJ48_t *motor) {
    if( motor->willPass == 0 ) return ;
    *motor->port = (*motor->port & (~motor->mask) ) | (motor->LUT & motor->mask) ;
    if(motor->willPass<0) {
        motor->LUT = (motor->LUT>>28) | (motor->LUT<<4) ;
        ++motor->willPass ;
        --motor->x ;
    } else {
        motor->LUT = (motor->LUT<<28) | (motor->LUT>>4) ;
        --motor->willPass ;
        ++motor->x ;
    }
}
#endif 

